%> @file cc_statespace_augmentation_for_disturbance_rejection.m
%> @brief State space matrices augmentation for the disturbance rejection.
%>
%> @author Mikhail Konnik
%> @date   11 January 2012
%>
%> @section distrubaug State augmentation for the disturbance rejection
%> In most implementations of MPC, the problem of the constant output disturbance is solved by incorporating a constant output disturbance into the process model. A constant output disturbance model can be constructed using the following augmented state-space model:
%> \f$ A = \left[ \begin{array}{cc}   A & 0\\   0 & I \\ \end{array}\right] \,\,\,  B = \left[ \begin{array}{c}   B\\   0 \\  \end{array}\right] \f$.
%>
%>
%> in which \f$p \in R^{sp}\f$ , \f$sp\f$ is the number of augmented output disturbance states, and Gp determines the effect of these states on the output.
%======================================================================
%> @param Ap			= discrete plant state evolution matrix.
%> @param Bp			= discrete plant input matrix.
%> @param Cp			= discrete plant output matrix.
%> @param Dp			= discrete plant feedthrough matrix.
%> @param Ad			= discrete disturbance state evolution matrix.
%> @param Bd			= discrete disturbance input matrix.
%> @param Cd			= discrete disturbance output matrix
%> @param Dd			= discrete disturbance feedthrough matrix.
%> @retval A			= augmentated discrete state evolution matrix A.
%> @retval B			= augmentated discrete input matrix B.
%> @retval C			= augmentated discrete output matrix C.
%> @retval D			= augmentated G matrix.
%> @retval flag_sparsemode	= spare or non-sparse matrices return.
% ======================================================================
function [A_e,B_e,C_e,G] = cc_statespace_augmentation_for_disturbance_rejection(Ap,Bp,Cp,Dp,Ad,Bd,Cd,Dd,flag_sparsemode)

switch flag_sparsemode
	case 0 %% This is FULL matrices, with lots of ZEROS

		[m1,c1]=size(Cp);
		[n1,n_in]=size(Bp);

		[dm1,dc1]=size(Cd);
		[dn1,dn_in]=size(Bd);

		[atm_A_rows,atm_A_cols] = size(Ad);   % number of rows in the Matrix Ad of the atmosphere

		A_e = [Ap, zeros(n1,atm_A_cols); ...
		zeros(atm_A_rows,n1), Ad];
		B_e = [Bp; zeros(atm_A_rows,n_in)];
		C_e = [Cp, Cd];

		G = [zeros(size(A_e,1)-size(Bd,1),n_in); Bd]; %%% noise matrix





	case 1 %% This is SPARSE matrices, with lots of ZEROS
		[m1,c1]=size(Cp);
		[n1,n_in]=size(Bp);

		[dm1,dc1]=size(Cd);
		[dn1,dn_in]=size(Bd);

		[atm_A_rows,atm_A_cols] = size(Ad);   % number of rows in the Matrix Ad of the atmosphere

		A_e = [Ap, sparse(n1,atm_A_cols); ...
		sparse(atm_A_rows,n1), Ad];
		B_e = [Bp; sparse(atm_A_rows,n_in)];
		C_e = [Cp, Cd];
		C_e = sparse(C_e); %%% Deliberately make the C_e matrix sparse!

		G = [sparse(size(A_e,1)-size(Bd,1),n_in); Bd]; %%% noise matrix




	otherwise
		fprintf('The flag_sparsemode is wrong! Only 1 or 0 are acceptable values! \n');
end %%%%% switch flag_sparsemode